﻿using System.Linq;
using Microsoft.VisualStudio.TestTools.UnitTesting;
using RaytracerLib;
using RaytracerLib.MathLib;
using System;
using TomanuExtensions;

namespace RaytracerTest
{
    [TestClass]
    public class Matrix4Test : TestBase
    {
        protected const float D3D_ROTATE_PRECISION = 2e-4f;

        [TestMethod]
        public void Matrix4_Test()
        {
            Matrix4_Rotate();
            Matrix4_Basic();
        }

        public void Matrix4_Basic() 
        {
            double[] ar = { 0, 1, -1, 12345656764564343, -1435434343543536e14 };

            int[] perm1 = new int[4];
            int[] perm2 = new int[4];

            do
            {
                var m1d = Microsoft.DirectX.Matrix.Zero;
                m1d.M11 = (float)ar[perm1[0]];
                m1d.M12 = (float)ar[perm1[1]];
                m1d.M13 = (float)ar[perm1[2]];
                m1d.M14 = (float)ar[perm1[3]];
                m1d.M21 = (float)ar[perm1[1]];
                m1d.M22 = (float)ar[perm1[2]];
                m1d.M23 = (float)ar[perm1[0]];
                m1d.M24 = (float)ar[perm1[3]];
                m1d.M31 = (float)ar[perm1[0]];
                m1d.M32 = (float)ar[perm1[3]];
                m1d.M33 = (float)ar[perm1[2]];
                m1d.M34 = (float)ar[perm1[1]];
                m1d.M41 = (float)ar[perm1[3]];
                m1d.M42 = (float)ar[perm1[1]];
                m1d.M43 = (float)ar[perm1[2]];
                m1d.M44 = (float)ar[perm1[0]];

                var m1 = new Matrix4(ar[perm1[0]], 
                                     ar[perm1[1]], 
                                     ar[perm1[2]], 
                                     ar[perm1[3]], 
                                     ar[perm1[1]], 
                                     ar[perm1[2]], 
                                     ar[perm1[0]], 
                                     ar[perm1[3]], 
                                     ar[perm1[0]], 
                                     ar[perm1[3]], 
                                     ar[perm1[2]], 
                                     ar[perm1[1]], 
                                     ar[perm1[3]], 
                                     ar[perm1[1]], 
                                     ar[perm1[2]], 
                                     ar[perm1[0]]);

                do
                {
                    var m2d = Microsoft.DirectX.Matrix.Zero;

                    m2d.M11 = (float)ar[perm2[0]];
                    m2d.M12 = (float)ar[perm2[1]];
                    m2d.M13 = (float)ar[perm2[2]];
                    m2d.M14 = (float)ar[perm2[3]];
                    m2d.M21 = (float)ar[perm2[1]];
                    m2d.M22 = (float)ar[perm2[2]];
                    m2d.M23 = (float)ar[perm2[0]];
                    m2d.M24 = (float)ar[perm2[3]];
                    m2d.M31 = (float)ar[perm2[0]];
                    m2d.M32 = (float)ar[perm2[3]];
                    m2d.M33 = (float)ar[perm2[2]];
                    m2d.M34 = (float)ar[perm2[1]];
                    m2d.M41 = (float)ar[perm2[3]];
                    m2d.M42 = (float)ar[perm2[1]];
                    m2d.M43 = (float)ar[perm2[2]];
                    m2d.M44 = (float)ar[perm2[0]];

                    var m2 = new Matrix4(ar[perm2[0]],
                                         ar[perm2[1]],
                                         ar[perm2[2]],
                                         ar[perm2[3]],
                                         ar[perm2[1]],
                                         ar[perm2[2]],
                                         ar[perm2[0]],
                                         ar[perm2[3]],
                                         ar[perm2[0]],
                                         ar[perm2[3]],
                                         ar[perm2[2]],
                                         ar[perm2[1]],
                                         ar[perm2[3]],
                                         ar[perm2[1]],
                                         ar[perm2[2]],
                                         ar[perm2[0]]);

                    // Add
                    {
                        var m3 = m1 + m2;
                        var m3d = m1d + m2d;
                        AreAlmostRelativeEquals(m3d, m3);
                    }
                    
                    // Sub
                    {
                        var m3 = m1 - m2;
                        var m3d = m1d - m2d;
                        AreAlmostRelativeEquals(m3d, m3);
                    }
                    
                    // Mul matrix
                    {
                        var m3 = m1 * m2;
                        var m3d = m1d * m2d;
                        AreAlmostRelativeEquals(m3d, m3);
                    }
                    
                    // Mul vector3
                    {
                        var vd = new Microsoft.DirectX.Vector4(
                            (float)m2[0], (float)m2[1], (float)m2[2], (float)m2[3]);
                        var v = new Vector3(m2[0], m2[1], m2[2]);
                        if ((v.Length != 0) && (m1[15] != 0) && (m2[3] == 1))
                        {
                            var r1 = m1 * (Vector3)v;
                            var r2 = Microsoft.DirectX.Vector4.Transform(vd,
                                Microsoft.DirectX.Matrix.TransposeMatrix(m1d));
                            var r3 = new Vector3(r1);
                            r2.X = r2.X / r2.W;
                            r2.Y = r2.Y / r2.W;
                            r2.Z = r2.Z / r2.W;
                            r2.W = 1;
                            AreAlmostRelativeEquals(r2, r3);
                        }
                    }
                    
                    // Transpose
                    {
                        var m3 = m1.Transposed;
                        var m3d = Microsoft.DirectX.Matrix.TransposeMatrix(m1d);
                        AreAlmostRelativeEquals(m3d, m3);
                    }

                    // Invert
                    {
                        if (m1.Determinant == 0)
                            continue;
                        var m3 = m1.Inverted;
                        var m3d = Microsoft.DirectX.Matrix.Invert(m1d);
                        AreAlmostRelativeEquals(m3d, m3);
                        AreAlmostRelativeEquals(m3d * m1d, Matrix4.Identity);
                        AreAlmostRelativeEquals(Matrix4.Identity, m3 * m1);
                    }
                    
                    // Determinant
                    {
                        var m3 = m1.Determinant;
                        var m3d = m1d.Determinant;
                        AreAlmostRelativeEquals(m3d, m3);
                    }
                    
                    // Equality.
                    {
                        bool b1 = m1d.Equals(m2d);
                        bool b2 = m1d == m2d;
                        bool b3 = m1.Equals(m2);
                        bool b4 = m1 == m2;
                    
                        Assert.IsTrue(b1 == b2);
                        Assert.IsTrue(b2 == b3);
                        Assert.IsTrue(b3 == b4);
                    }
                    
                    // Translation
                    {
                        var r1 = Matrix4.CreateTranslation(m2[0], m2[1], m2[2]);
                        var r2 = Matrix4.CreateTranslation(
                            new Vector3(m2[0], m2[1], m2[2]));
                        var r5 = Microsoft.DirectX.Matrix.Translation(
                            (float)m2[0], (float)m2[1], (float)m2[2]);
                    
                        AreAlmostRelativeEquals(r1, r2);
                        AreAlmostRelativeEquals(r5, r2.Transposed);
                    }
                    
                    // Scale
                    {
                        var r1 = Matrix4.CreateScale(m2[0], m2[1], m2[2]);
                        var r2 = Matrix4.CreateScale(
                            new Vector3(m2[0], m2[1], m2[2]));
                        var r5 = Microsoft.DirectX.Matrix.Scaling(
                            (float)m2[0], (float)m2[1], (float)m2[2]);
                    
                        AreAlmostRelativeEquals(r1, r2);
                        AreAlmostRelativeEquals(r5, r2.Transposed);
                    }
                }
                while (TestBase.IncCounter(perm1, perm1.Length, ar.Length));
            }
            while (TestBase.IncCounter(perm2, perm2.Length, ar.Length));

            // Equal.
            {
                var m1 = new Matrix4(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
                var m2 = new Matrix4(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
                var ar1 = m1.ToArray();

                Assert.IsTrue(m1.Equals(m1));

                for (int x = 0; x < 3; x++)
                {
                    for (int y = 0; y < 3; y++)
                    {
                        ar1[x + y * 3] = 1;
                        m1 = new Matrix4(ar1);

                        Assert.IsFalse(m1 == m2);
                        Assert.IsTrue(m1 != m2);
                        Assert.IsFalse(m1.Equals(m2));

                        m2 = new Matrix4(ar1);

                        Assert.IsTrue(m1 == m2);
                        Assert.IsFalse(m1 != m2);
                        Assert.IsTrue(m1.Equals(m2));

                        ar1[x + y * 3] = 0;
                        m1 = new Matrix4(ar1);
                        m2 = new Matrix4(ar1);
                    }
                }
            }
        }

        public void Matrix4_Rotate()
        {
            double[] ar = { 0, -1, 1, 0.5, -0.5, 1.0 / 3, -1.0 / 3, 0.1, -0.1, 2, -2,
                           10, -10 };
            double[] angles = { 0, 90, -90, 180, -180, 270, -270, 360, -360, 720,
                               -720, 450, -450, 10, -10, 115, -115, 85, -85, 165,
                               -165, 190, -190, 260, -260, 280, -280, 350, -350,
                               370, -370 };

            var perm = from v1 in ar
                       from v2 in ar
                       from v3 in ar
                       from angle in angles
                       select new
                       {
                           v1,
                           v2,
                           v3,
                           angle = MathExtensions.ToRad(angle)
                       };

            foreach (var p in perm)
            {
                // rotate X
                {
                    var m1 = Microsoft.DirectX.Matrix.RotationX(-(float)p.angle);
                    var m2 = Matrix4.CreateRotateX(MathExtensions.ToDeg(p.angle));
                    AreAlmostRelativeEquals(m1, m2);
                }
                
                // rotate Y
                {
                    var m1 = Microsoft.DirectX.Matrix.RotationY(-(float)(p.angle));
                    var m2 = Matrix4.CreateRotateY(MathExtensions.ToDeg(p.angle));
                    AreAlmostRelativeEquals(m1, m2);
                }
                
                // rotate Z
                {
                    var m1 = Microsoft.DirectX.Matrix.RotationZ(-(float)(p.angle));
                    var m2 = Matrix4.CreateRotateZ(MathExtensions.ToDeg(p.angle));
                    AreAlmostRelativeEquals(m1, m2);
                }
                
                // rotate around vector #1
                {
                    var v1d = new Microsoft.DirectX.Vector3((float)p.v1, (float)p.v2,
                        (float)p.v3);
                    var md = Microsoft.DirectX.Matrix.RotationAxis(v1d, (float)p.angle);
                
                    var v1 = new Vector3(p.v1, p.v2, p.v3);
                    var m = Matrix4.CreateRotateAroundVector(v1, MathExtensions.ToDeg(p.angle));
                
                    AreAlmostRelativeEquals(md, m.Transposed);
                }

                // rotate around vector #2
                {
                    var v1d = new Microsoft.DirectX.Vector3((float)p.v1, (float)p.v2,
                        (float)p.v3);
                    var md = Microsoft.DirectX.Matrix.RotationAxis(v1d, (float)p.angle);

                    var v1 = new Vector3(p.v1, p.v2, p.v3);
                    var m = Matrix4.CreateRotateAroundVector(v1, Math.Cos(p.angle), Math.Sin(p.angle));

                    AreAlmostRelativeEquals(md, m.Transposed);
                }
                
                // matrix decomposition
                {
                    var v1 = new Vector3(p.v1, p.v2, p.v3);
                    var m = Matrix4.CreateRotateAroundVector(v1, MathExtensions.ToDeg(p.angle));
                
                    var m3 = m.Rotation;
                
                    Assert.IsTrue(Matrix4.Identity.IsAlmostRelativeEquals(m.Translation));
                
                    var t = new Vector3((p.v1 + 4) * p.v2, (p.v2 + 6) * p.v3,
                        (p.v3 - 4) * p.v1);
                    m = m * Matrix4.CreateTranslation(t);
                
                    Assert.IsTrue(Matrix4.CreateTranslation(t).IsAlmostRelativeEquals(m.Translation));
                    Assert.IsTrue(m.Rotation.Equals(m3));
                }
            }
        }
    }
}